#ifndef ANDRES_SIGN_POLE_MATCHING_SPM_H
#define ANDRES_SIGN_POLE_MATCHING_SPM_H

#include <base/defines.h>
#include <base/camera.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <Eigen/Core>

class DbgFrame;

namespace SPM
{
    class SignPoleMatching
    {
    public:
        SignPoleMatching();

        bool Calculate_Pose(
            const DlFrame &dl,
            const std::vector<hdmap_sign>& hdmap_signs,
			Pose &pose,
			Camera &cam,
			double &rmse,
			DbgFrame* dbg_frame);
    protected:
        std::vector<std::pair<hdmap_sign, envelope2D>> ProjectHDMap(
            const Camera &cam,
            const Pose& pose, 
            const std::vector<hdmap_sign>& hdmap_signs);

        void cal_KM_cost_parallel(
            std::vector<std::vector<float>> &_KM_cost,
            const std::vector<std::pair<hdmap_sign, envelope2D>>& hd_signs, 
            const std::vector<DlSign>& dl_signs);

    private:
        bool debug_message_;
        float geometric_CRITERIA_;

    };
}

#endif